#include "zf_common_headfile.h"
#include "beacon.h"
#include "stdlib.h"
#include "math.h"

float Beacon_X,Beacon_Y;//信标灯的中心坐标
int blackflag = 1;
unsigned int left=1, right=MT9V03X_W-1, top=1, below=MT9V03X_H-1;//扫描窗口初始化

unsigned char Threshold=140;//全局阈值

uint8 mt9v03x_image_Bin[MT9V03X_H][MT9V03X_W];//二值化后

void SeekBeacon(void)//放在总函数里的
{
if(mt9v03x_finish_flag)
	{
		Binaryzation(mt9v03x_image,mt9v03x_image_Bin);
		
		//Two_Pass_V1(); 
				
		Scan_Window_Return_XY();
		
		led_rvs;
		
		mt9v03x_finish_flag = 0;//标志位清零
	}
}


void show_raw_image(void)
{
		if(mt9v03x_finish_flag)
	{
        
        tft180_show_gray_image(0, 0, mt9v03x_image[0], MT9V03X_W, MT9V03X_H,160,120, 0);
        led_rvs;//完成一次采集
		mt9v03x_finish_flag = 0;//标志位清零	
    }
}

void show_processed_image (void)//在menu里操作
{
	 char txt[16];

		if(mt9v03x_finish_flag)
	{
        
        tft180_show_gray_image(0, 0, mt9v03x_image[0], MT9V03X_W, MT9V03X_H,160,120, Threshold);
        
		Binaryzation(mt9v03x_image,mt9v03x_image_Bin);//全局固定阈值

		//Two_Pass_V1(); 
		
		Scan_Window_Return_XY();
		
		sprintf((char*)txt,"X:%3.1f",Beacon_X);
		tft180_show_string(0,88,txt);
	    
		sprintf((char*)txt,"Y:%3.1f",Beacon_Y);
		tft180_show_string(0,104,txt);

		/*这俩显示出来一个十字线，十字中心为信标灯*/
		tft180_draw_line(Beacon_X,0,Beacon_X,120,RGB565_RED);//竖线
		tft180_draw_line(0,Beacon_Y,159,Beacon_Y,RGB565_RED);//横线

//		/*显示扫描窗口*/
//		tft180_draw_line(left,top,right,top,RGB565_GREEN);
//		tft180_draw_line(left,below,right,below,RGB565_GREEN);
//		tft180_draw_line(left,top,left,below,RGB565_GREEN);
//		tft180_draw_line(right,top,right,below,RGB565_GREEN);

		led_rvs;//完成一次采集

		mt9v03x_finish_flag = 0;//标志位清零	
		}
}

/**
 * @brief Two-pass-V1
 * @note 实现了找到一张图中，第一个长大于宽的连通域
 * @author 冯宇轩
 */
struct lbl_T
{
	int data;
	struct lbl_T *next;
}label_T[500];//并查集标签树

void Two_Pass_V1(void)
{
	int i, j, k;//计数
	int Simu_Pointer = 0;//label_T模拟指针
	int label = 2;//标签号 
	int ld_count = 1;//ld数组有效数据计数器
	int ld_flag = 1;//第二次遍历判断标志位
	int t = MT9V03X_H, b = 0, l = MT9V03X_W, r = 0;//边框坐标范围(t为顶边b为底边l为左侧边r为右侧边)
	int sum = 0;//记录连通域大小	
	int ld[50]={999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999,
				999,999,999,999,999}; //标签数据初始化数组

	struct lbl_T temp;//查询根节点时所用临时结构体
	temp.data = 0;
	temp.next = NULL;

	if (blackflag == 0)
	{
		//边框清零 否则算法过程中会出现bug
		for (i = 0; i < MT9V03X_H; ++i)//左右清0
		{
			mt9v03x_image_Bin[i][0] = 0;
			mt9v03x_image_Bin[i][MT9V03X_W - 1] = 0;
		}
		for (j = 0; j < MT9V03X_W; ++j)//上下清0
		{
			mt9v03x_image_Bin[0][j] = 0;
			mt9v03x_image_Bin[MT9V03X_H - 1][j] = 0;
		}
		
		//第一次遍历
		for (i = 1; i < MT9V03X_H - 1; ++i)
		{
			for (j = 1; j < MT9V03X_W - 1; ++j)
			{
				if (mt9v03x_image_Bin[i][j] == 1 && mt9v03x_image_Bin[i - 1][j] < 1 && mt9v03x_image_Bin[i][j - 1] < 1)
				{
					mt9v03x_image_Bin[i][j] = label;
					label_T[Simu_Pointer].data = label;
					label_T[Simu_Pointer].next = NULL;
					Simu_Pointer++;
					label++;
				}
				if (mt9v03x_image_Bin[i][j] == 1 && (mt9v03x_image_Bin[i - 1][j] >= 2 || mt9v03x_image_Bin[i][j - 1] >= 2))
				{
					if (mt9v03x_image_Bin[i - 1][j] >= 2)
						mt9v03x_image_Bin[i][j] = mt9v03x_image_Bin[i - 1][j];
					if ((mt9v03x_image_Bin[i - 1][j] < 2) && (mt9v03x_image_Bin[i][j - 1] >= 2))
						mt9v03x_image_Bin[i][j] = mt9v03x_image_Bin[i][j - 1];
					if (mt9v03x_image_Bin[i - 1][j] >= 2 && mt9v03x_image_Bin[i][j - 1] >= 2 && mt9v03x_image_Bin[i - 1][j] < mt9v03x_image_Bin[i][j - 1])
						label_T[(mt9v03x_image_Bin[i][j - 1] - 2)].next = &label_T[(mt9v03x_image_Bin[i - 1][j] - 2)];
				}
			}
		}
		
		//第二次遍历
		for (i = 1; i < MT9V03X_H - 1; ++i)
		{
			for (j = 1; j < MT9V03X_W - 1; ++j)
			{
				if (mt9v03x_image_Bin[i][j] > 1)
				{
					temp = label_T[(mt9v03x_image_Bin[i][j] - 2)];
					while(temp.next != NULL)
					{
						temp = *temp.next;//将根节点的数据赋给该结构体
					}
					mt9v03x_image_Bin[i][j] = temp.data;
					for (k = 0; k < ld_count; ++k)
					{
						if (temp.data == ld[k])//判断ld数组中有无当前label值
							ld_flag = 0;
					}
					if (ld_flag)
					{
						ld[ld_count - 1] = temp.data;//将不重复的label值录入ld数组
						ld_count++;
					}
					ld_flag = 1;
				}
			}
		}
		
		//遍历每一个连通域
		for (k = 0; k < ld_count; ++k)
		{
			t = MT9V03X_H; b = 0; l = MT9V03X_W; r = 0; sum = 0;
			//确定当前连通域边界
			for (i = 0; i < MT9V03X_H; ++i)
			{
				for (j = 0; j < MT9V03X_W; ++j)
				{
					if (mt9v03x_image_Bin[i][j] == ld[k])
					{
						if (l > j)
							l = j;
						if (r < j)
							r = j;
						if (t > i)
							t = i;
						if (b < i)
							b = i;
							sum++;
					}
				}
			}
			
			//连通域特征判断
			if (r - l > b - t)//长大于宽
			{
				//扫描窗口框选该连通域
				right = r;
				left = l;
				top = t;
				below = b;
				break;
			}		
			else continue;
		 }
		
		//扫描窗口内二值化
		for (i = top; i <= below; i++)
		{
			for (j = left; j <= right; j++)
			{
				if (mt9v03x_image_Bin[i][j] != 0)
					mt9v03x_image_Bin[i][j] = 1;
			}
		}
		blackflag = 1;
	}
}

/*
 * @brief这个函数确定了扫描窗口的中心值
 * @note采用的中心确定方式为，二值化为1像素点的平均XY坐标 
 * 
 */
void Scan_Window_Return_XY(void)
{
unsigned int i, j;
unsigned int num = 0;
float w_sum = 0, h_sum = 0;
	
	for (i = top; i <= below; i++)
	{
		for (j = left; j <= right; j++)
		{
			if(mt9v03x_image_Bin[i][j]!=0)
			//if ((mt9v03x_image_Bin[i][j] + mt9v03x_image_Bin[i+1][j] + mt9v03x_image_Bin[i-1][j] + mt9v03x_image_Bin[i][j+1] + mt9v03x_image_Bin[i][j-1]) > 3)
			{
				num++;
				h_sum += i;
				w_sum += j;
			}
		}
	}
	Beacon_Y=h_sum/num;
	Beacon_X=w_sum/num;
}


void Binaryzation(unsigned char imageIn[MT9V03X_H][MT9V03X_W], unsigned char imageOut[MT9V03X_H][MT9V03X_W])//全局固定阈值
{
 unsigned char i,j;//i为行标，j为列标	
 for (j = 0; j < MT9V03X_W; ++j)//先对列进行扫描
	{
		for (i = 0; i < MT9V03X_H; ++i)//然后对行进行扫描
			{
			  if (mt9v03x_image[i][j] >= Threshold) 
					mt9v03x_image_Bin[i][j] = 1; 
			  else
			   mt9v03x_image_Bin[i][j] = 0;
			}
	}
	blackflag = 0;
}

